#ifndef ros2_SLAMWEBSOCKET_QNODE_HPP_
#define ros2_SLAMWEBSOCKET_QNODE_HPP_

/*****************************************************************************
** Includes
*****************************************************************************/
//ROS2
#include "rclcpp/rclcpp.hpp"
#include "pccmd_msg/msg/pc_cmd.hpp"
#include "slamready_msg/msg/slam_ready.hpp"
#include "slampos_msg/msg/slam_pos.hpp"

//CPP
#include <chrono>
#include <memory>
#include <functional>
#include <string>

class SlamWebSocket;
  /*****************************************************************************
  ** Class
  *****************************************************************************/

  class RobotCmdNode : public rclcpp::Node
  {
  public:
      RobotCmdNode(SlamWebSocket* ws);
      virtual ~RobotCmdNode();

      void pubSlamReady();

      void pubSlamPos(float x, float y, float z, float rx, float ry, float rz);

  private:

    void topic_callback(const pccmd_msg::msg::PcCmd::SharedPtr msg) const;

    rclcpp::Subscription<pccmd_msg::msg::PcCmd>::SharedPtr subscription_;

    rclcpp::Publisher<slamready_msg::msg::SlamReady>::SharedPtr m_pubSLAMReady;

    rclcpp::Publisher<slampos_msg::msg::SlamPos>::SharedPtr m_pubSLAMPos;

    SlamWebSocket* m_ws;
  };

#endif /* ros2_SLAMWEBSOCKET_QNODE_HPP_ */
